package edu.gatech.cc.liam.geometry.linear;
import java.text.DecimalFormat;
import java.util.Arrays;
import java.util.Collection;

import edu.gatech.cc.liam.core.Globals;
import edu.gatech.cc.liam.geometry.linear.convexhull.Preferences;

// TODO change double[] inputs into current framework 

public class Halfspace {
	public double[] normal;
	public double offset;

	// Ax1 + Bx1 ... + C <= 0
	public Halfspace() {}
	
	public Halfspace(double[] normalOffset) {
		this.normal = Arrays.copyOf(normalOffset, normalOffset.length-1);
		this.offset = normalOffset[normalOffset.length-1];
	}
	public Halfspace(double[] normal, double offset) {
		this.normal = normal;
		this.offset = offset;
	}
	public Halfspace(NPoint np, double offset) {
		this.normal = np.values.clone();
		this.offset = offset;
	}
	
	public static Halfspace makeAgtb(double[] A, double b) {
		NPoint n = new NPoint(A);
		n.scale(-1.0);
		return new Halfspace(n, b);
	}
	public static Halfspace makeAltb(double[] A, double b) {
		return new Halfspace(A, -b);
	}
	
	/**
	 * @param vertices a set of points that must be satisfied by the halfspace
	 * @return the halfspace uniquely satisfying the vertices
	 * such that the +infinity vector (in all dimensions) is feasible, null otherwise
	 */
	public static Halfspace makeHalfspaceFromPoints(PointSet vertices) {
		PointSet points = vertices.getPossibleHalfspacesFromPoints();
		if(points.size() > 2)
			return null;
		return makeHalfspaceFromLowestOrthantPoint(points);
	}
	
	private static Halfspace makeHalfspaceFromLowestOrthantPoint(PointSet thePoints) {
		int dimensions = thePoints.iterator().next().dimensions() - 1;
//		if (thePoint.size() > 2)
//			return null;
		NPoint normalOffset = thePoints.iterator().next();
		for(int i=0; i<normalOffset.dimensions(); i++) {
			if(normalOffset.values[i] != 0.0) {
				if(normalOffset.values[i] < 0.0)
					normalOffset.scale(-1.0);
				break;
			}
		}
		assert normalOffset.dimensions() == dimensions+1;
		double[] normal = Arrays.copyOf(normalOffset.values, dimensions);
		assert normal != new double[normal.length];
		//double offset = normalOffset.values[dimensions];
		return new Halfspace(normalOffset.values);
	}
	
	/**
	 * The distance of a point from the plane. A negative value means the point is feasible.
	 * @param thePoint
	 * @return
	 */
	public double pointDistanceFromHyperplane(NPoint thePoint) {
		return pointDistanceFromHyperplane(thePoint.values);
	}
	public double pointDistanceFromHyperplane(double[] thePoint) {
		double sum = dotWithNormal(thePoint);
		sum += offset;
		return sum;
	}
	
	public boolean isFeasible(NPoint aPoint) {
		return pointDistanceFromHyperplane(aPoint) <= Preferences.TOLERANCE;
	}
	
	public double dotWithNormal(NPoint aPoint) {
		return dotWithNormal(aPoint.values);
	}
	public double dotWithNormal(double[] otherVec) {
		double sum = 0.0;
		for(int i=0; i<otherVec.length; i++) {
			sum += normal[i] * otherVec[i];
		}
		return sum;
	}
	
	public void normalize() {
		double scale = new NPoint(normal.length, 0.0).distance(new NPoint(normal));
		for(int i=0; i<normal.length; i++)
			normal[i] /= scale;
		offset /= scale;
	}
	
	// checks to see if the halfspace is either a truism (nonpositive <= 0)
	// or if it is always false (positive <= 0)
	public boolean isDegenerate() {
		for(int i=0; i<normal.length; i++)
			if(Math.abs(normal[i]) >= Preferences.TOLERANCE)
				return false;
		return true;
	}
	public boolean isInfeasible() {
		return (isDegenerate() && offset >= Preferences.TOLERANCE);
	}
	
	@Override
	public String toString() {
		DecimalFormat printFormat = new DecimalFormat("0.0####");
		String rStr = "{norm:(" + printFormat.format(this.normal[0]);
		for(int i=1; i<normal.length; i++) {
			rStr += "," + printFormat.format(this.normal[i]);
		}
		rStr += "), off:" + printFormat.format(offset) + "}";
		return rStr;
	}
	
	private String getFixedLengthDecimalFormal(double d, DecimalFormat df, int paddedLength) {
		String ds = df.format(d);
		if(ds.length() > paddedLength)
			paddedLength = ds.length();
		char[] rString = new char[paddedLength];
		Arrays.fill(rString, ' ');
		ds.getChars(0, ds.length(), rString, (paddedLength-ds.length()));
		return String.valueOf(rString);
	}
	
	public String toStringFixedLength(int leadingDigits) {
		DecimalFormat normalPrintFormat = new DecimalFormat("#.000");
		int normalPaddingSize = 7; // including negative sign and decimal place + whitespace
		
		String rStr = "";
		for(double d : this.normal) {
			rStr += getFixedLengthDecimalFormal(d, normalPrintFormat, normalPaddingSize);
			rStr += " ";
		}
		rStr += "<= ";
		DecimalFormat offsetPrintFormat = new DecimalFormat("#.00");
		int offsetPaddingSize = leadingDigits + 5;
		rStr += getFixedLengthDecimalFormal(-this.offset, offsetPrintFormat, offsetPaddingSize);
		rStr += "\n";
		return rStr;
	}
	
	/***
	 * 
	 * @param numDimensions
	 * @return A hyperplane with a random unit normal and a random offset between 0 and 1
	 */
	public static Halfspace makeRandomHalfspace(int numDimensions) {
		Halfspace theHyper = new Halfspace();
		theHyper.normal = NPoint.makePolarRandom(numDimensions).values;
		theHyper.offset = -Globals.rand.nextDouble();
		return theHyper;
	}
	
	@Override
	protected Halfspace clone() {
		return new Halfspace(this.normal.clone(), this.offset);
	}

	@Override
	public int hashCode() {
		final int prime = 31;
		int result = 1;
		result = prime * result + Arrays.hashCode(normal);
		long temp;
		temp = Double.doubleToLongBits(offset);
		result = prime * result + (int) (temp ^ (temp >>> 32));
		return result;
	}

	@Override
	public boolean equals(Object obj) {
		if (this == obj)
			return true;
		if (obj == null)
			return false;
		if (getClass() != obj.getClass())
			return false;
		Halfspace other = (Halfspace) obj;
		if (!Arrays.equals(normal, other.normal))
			return false;
		if (Double.doubleToLongBits(offset) != Double
				.doubleToLongBits(other.offset))
			return false;
		return true;
	}
	
	public boolean softEquals(Halfspace h) {
		if( ! new NPoint(this.normal).softEquals(new NPoint(h.normal)) )
			return false;
		if( Math.abs(this.offset - h.offset ) > Globals.TOLERANCE)
			return false;
		return true;
	}
	
	public double[] toArray() {
		double[] r = Arrays.copyOf(this.normal, this.normal.length+1);
		r[r.length-1] = this.offset;
		return r;
	}
}
